#include "port.h"

/**
 * @brief: Initialize the stack of a newly created task. 
 * 
 */
OSStackType *StkInit(OSStackType *tos, task function, void *args, OSStackType stack_size)
{
	/* Simulate the stack frame as it was one of an interrupted task */
	#if STACK_GROWTH == 1
	
	OSStackType *stk;
  	stk = tos;//this causes #bug 0: bos + stack_size - 1;
	*--stk = 0x01000000; //xPSR Reserved, value 1 is for the t-bit
	
	*--stk = (uint_32)function;  //PC reserved
	*--stk = 0; //  LR reserved
	
	stk -= 4; // R12, R3, R2, R1
	*--stk = (uint_32)args; // r0 -- function arguments
	stk -= 8; // r4 - r11, space is reserved.


	return stk;
	#endif

}


void OSTickStart(void){
  SysTick->CTRL  = SysTick_CTRL_CLKSOURCE_Msk | 
                   SysTick_CTRL_TICKINT_Msk   | 
                   SysTick_CTRL_ENABLE_Msk;                    /* Enable SysTick IRQ and SysTick Timer */
}

uint_32 OSTickConfig(void){
  unsigned int ticks = CPU_FREQ / OS_TICK_FREQ;
  if (ticks > SysTick_LOAD_RELOAD_Msk)  return (1);            /* Reload value impossible */
                                                               
  SysTick->LOAD  = (ticks & SysTick_LOAD_RELOAD_Msk) - 1;      /* set reload register */
  NVIC_SetPriority (SysTick_IRQn, (1<<__NVIC_PRIO_BITS) - 1);  /* set Priority for Cortex-M0 System Interrupts */
  SysTick->VAL   = 0;                                          /* Load the SysTick Counter Value */
  return (0);               
}	


#ifdef __DEBUG__
volatile uint32_t r0;
volatile uint32_t r1;
volatile uint32_t r2;
volatile uint32_t r3;
volatile uint32_t r12;
volatile uint32_t lr; /* Link register. */
volatile uint32_t pc; /* Program counter. */
volatile uint32_t psr;/* Program status register. */

void prvGetRegistersFromStack( uint32_t *pulFaultStackAddress )
{
/* These are volatile to try and prevent the compiler/linker optimising them
away as the variables never actually get used.  If the debugger won't show the
values of the variables, make them global my moving their declaration outside
of this function. */

    r0 = pulFaultStackAddress[ 0 ];
    r1 = pulFaultStackAddress[ 1 ];
    r2 = pulFaultStackAddress[ 2 ];
    r3 = pulFaultStackAddress[ 3 ];

    r12 = pulFaultStackAddress[ 4 ];
    lr = pulFaultStackAddress[ 5 ];
    pc = pulFaultStackAddress[ 6 ];
    psr = pulFaultStackAddress[ 7 ];

    /* When the following line is hit, the variables contain the register values. */
    for( ;; );
}
#endif





/*__asm void FindHighestPriorityTask()
{
	PRESERVE 8
	IMPORT ReadyTaskBitmap
	IMPORT highestTCB
	IMPORT ReadyTaskArray
	ldr r0, =ReadyTaskBitmap
	ldr r0, [r0]
	rbit r1, r0
	clz r0, r1
	ldr r1, =ReadyTaskArray
	ldr r2, [r1]
	ldr r0, [r2 + r]
}*/
